#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
// #include <std_msgs/msg/float32_multi_array.h>
// #include <std_msgs/msg/int16_multi_array.h>
// #include<std_msgs/msg/detail/byte_multi_array__functions.h>
#include<std_msgs/msg/float32.h>
#include <Esp32McpwmMotor.h>
#include "PidController.h"
#include<Ticker.h>

Esp32McpwmMotor espMotor; // 创建一个名为espMotor的对象，用于控制电机
#define enA1 4
#define enB1 5
#define enA2 18
#define enB2 19
#define IN1 33
#define IN2 26
#define ENA 25
#define IN3 27
#define IN4 12
#define ENB 14

float Kp=0.05, Ki=0.5, Kd=0.0;
//设定值Setpoint，测速值Input，pid之后的输出值Output
float Setpoint1=0.0, Input1=0.0, Output1=0.0;
float Setpoint2=0.0, Input2=0.0, Output2=0.0;
float d=0.07;//车轮直径70mm

Ticker timer_read_encoder;  // 中断函数定时器定义

PidController pid_controller[2];   // 创建PidController的两个对象

int16_t sENA=0, sENB=0;//占空比-100~100
// const unsigned int interrupt_time=200;
int interrupt_time=80;

void motorOUT(void);
void READ_ENCODER_1(void);
void READ_ENCODER_2(void);
void update_parameter(void);
void readVel(void);

unsigned long preTime=0;
int32_t enL,enR;
float motorLOOP=16.0*56.0;//线数x减速比
float velL,velR;

rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
// rcl_timer_t timer;

// 声明话题订阅者
rcl_subscription_t subscriberL;
rcl_subscription_t subscriberR;
// 声明话题发布者
rcl_publisher_t publisherL;
rcl_publisher_t publisherR;
// 声明消息文件
std_msgs__msg__Float32 subL_msg;
std_msgs__msg__Float32 subR_msg;
std_msgs__msg__Float32 pubL_msg;
std_msgs__msg__Float32 pubR_msg;

// 定义话题L接收回调函数
void callback_subscription_L(const void *msgin)
{
  Serial.println("subL get in");
  const std_msgs__msg__Float32 * msg =(const std_msgs__msg__Float32 *)msgin;
    Setpoint1=msg->data;
    Serial.print("Setpoint1=");
    Serial.print(Setpoint1);
    pid_controller[0].update_target(Setpoint1); // 更新控制器的目标值
}
// 定义话题R接收回调函数
void callback_subscription_R(const void *msgin)
{
  Serial.println("subR get in");
  const std_msgs__msg__Float32 * msg =(const std_msgs__msg__Float32 *)msgin;
    Setpoint2=msg->data;
    Serial.print("Setpoint2=");
    Serial.print(Setpoint2);
    pid_controller[1].update_target(Setpoint2); // 更新控制器的目标值
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial2.begin(115200);
  while (!Serial);
  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial2);
  // 延时时一段时间，等待设置完成
  delay(2000);
  // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点 vehicle
  rclc_node_init_default(&node, "vehicle", "", &support);
  // 订阅者L初始化
  rclc_subscription_init_default(
    &subscriberL,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs,msg,Float32),
    "cal_vel_l"
  );
  // 订阅者R初始化
  rclc_subscription_init_default(
    &subscriberR,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs,msg,Float32),
    "cal_vel_r"
  );
  //发布者L初始化
  rclc_publisher_init_default(
    &publisherL,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
    "esp_pro_l"
  );
  //发布者R初始化
  rclc_publisher_init_default(
    &publisherR,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
    "esp_pro_r"
  );
  // 创建执行器
  rclc_executor_init(&executor, &support.context, 4, &allocator);
  // 为执行器添加一个订阅者
  rclc_executor_add_subscription(&executor, &subscriberL, &subL_msg, &callback_subscription_L, ON_NEW_DATA);
  rclc_executor_add_subscription(&executor, &subscriberR, &subR_msg, &callback_subscription_R, ON_NEW_DATA);
  // 给执行器添加定时器
  // rclc_executor_add_timer(&executor, &timer);
  Serial.println("Micro-ROS Initialization complete"); // 添加调试信息
  //
  //
  espMotor.attachMotor(0,IN1,IN2);
  espMotor.attachMotor(1,IN4,IN3);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  pinMode(enA1,INPUT);
  pinMode(enB1,INPUT);
  pinMode(enA2,INPUT);
  pinMode(enB2,INPUT);
  //编码器接口1 开启外部跳边沿中断 
  attachInterrupt(enA1,READ_ENCODER_1,CHANGE);//中断函数READ_ENCODER_1
  //编码器接口2 开启外部跳边沿中断 
  attachInterrupt(enA2,READ_ENCODER_2,CHANGE);//中断函数READ_ENCODER_2
  interrupts();//打开外部中断
  update_parameter();
  preTime=0;
  //
  timer_read_encoder.attach_ms(interrupt_time,readVel);
}

void loop() {
  // put your main code here, to run repeatedly:
  // 循环处理数据
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  //update_parameter();
  pubL_msg.data=Input1;
  pubR_msg.data=Input2;
  rcl_ret_t ret=rcl_publish(&publisherL, &pubL_msg, NULL);
  ret=rcl_publish(&publisherR, &pubR_msg, NULL);
  delay(interrupt_time);
}

void update_parameter(){
  pid_controller[0].update_pid(Kp, Ki, Kd);
  pid_controller[1].update_pid(Kp, Ki, Kd);
  pid_controller[0].update_target(Setpoint1); // 更新控制器的目标值
  pid_controller[1].update_target(Setpoint2);
  pid_controller[0].out_limit(-100.0, 100.0);
  pid_controller[1].out_limit(-100.0, 100.0);
}

void motorOUT(){
  sENA=(int16_t)Output1;
  sENB=(int16_t)Output2;
  espMotor.updateMotorSpeed(0,sENA);
  espMotor.updateMotorSpeed(1,sENB);
}

void READ_ENCODER_1(void){
  if (digitalRead(enA1)==LOW)
  {
    if (digitalRead(enB1)==LOW)
    {
      enL--;
    }
    else
    {
      enL++;
    }
  }
}

void READ_ENCODER_2(void){
  if (digitalRead(enA2)==LOW)
  {
    if (digitalRead(enB2)==LOW)
    {
      enR++;
    }
    else
    {
      enR--;
    }
  }
}

void readVel(){
  // unsigned long nowTime=millis();
  // unsigned long timeP=nowTime-preTime;
  float enLf=(float)enL;
  float enRf=(float)enR;
  enL=0;
  enR=0;
  float in_time=((float)interrupt_time)/1000.0;
  Input1=(enLf/motorLOOP)/in_time*60.0;
  Input2=(enRf/motorLOOP)/in_time*60.0;
  // 计算最新的电机输出值
  if (abs(Setpoint1)>0.001)
  {
    Output1 = pid_controller[0].update(Input1);
  }
  else
  {
    Output1 = 0.0;
  }
  if (abs(Setpoint2)>0.001)
  {
    Output2 = pid_controller[1].update(Input2);
  }
  else
  {
    Output2 = 0.0;
  }
  motorOUT();
}